/*
veronda material


W(I1, I3) =  1/2 (mu/gamma (exp (gamma(I1 -3)) - 1)
- mu ln(I3)
+ lambda  (sqrt(I3) - 1)^2)

S = 2 DW 
= mu exp (gamma (I1-3)) I - mu C^-1 + lambda (sqrt(I3) - 1)sqrt(I3) C^-1
= mu exp (gamma (I1-3)) I + (lambda (sqrt(I3) - 1) sqrt(I3) - mu) C^-1

DS : H  =
gamma mu exp(gamma (I3-3)) tr(H) I
+ mu (C^-1 H C^-1)
+ (2sqrt(I3-1) lambda/2 sqrt(I3) (C^-1 : H) C^-1
- (I3 -sqrt(I3)) lambda C^-1 H C^-1

= gamma mu exp(gamma (I3-3)) tr(H) I +
([(I3 -sqrt(I3)) lambda - mu ] C^-1 H  + 2sqrt(I3-1) lambda/2 sqrt(I3) (C^-1 : H)) C^-1

*/


#include <cmath>

#include "TetrahedronState.h"
#include "Matrix.h"
#include "Mechanics.h"



Real ConsistentVerondaElasticEnergy (Real I1, Real I2, Real I3)
{
	// todo : gamma = 0.
	return 0.5* (lame_mu/lame_gamma * (exp (lame_gamma*(I1 -3.)) - 1.)
		- lame_mu *log(I3)
		+ lame_lambda*  Sqr (sqrt(I3) - 1.0));
}


/*
(+
Matrix3::multiply_mm 
Matrix3::add_diag 
Matrix3::square 
Matrix3::determinant 
1 Matrix3::trace 250
Matrix3::invert_to_with_det 
Matrix3::diag 
Matrix3::saxpy 
Matrix3::multiply_mm
Matrix3::multiply_mm
Matrix3::scale )

*/
int ConsistentVerondaElasticForce (TetrahedronState const*me,
								  Matrix3 *force_mat,
								  Matrix3 const &disp_mat)
{
	Matrix3 C;
	Matrix3 C_inv;
	Matrix3 F;

	Matrix3::MultiplyMM (F, disp_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	Real J = F.Determinant ();
	Real I3 = Sqr(J);
	Real I1 = C.Trace ();
	Real exponent = exp (lame_gamma * (I1 - 3));

	Matrix3::InvertToWithDet (C_inv, C, I3);

	Matrix3 S;
	S.Diag (lame_mu*  exponent);

	Matrix3::Saxpy (S, lame_lambda * (I3-1)*I3 - lame_mu, C_inv, S);

	*force_mat = F * S * me->invloc_transpose_;
	Matrix3::Scale (*force_mat , -1.0, *force_mat);

	return 530;
}


/*
(+
Matrix3::multiply_mm 
Matrix3::add_diag 
Matrix3::square 
Matrix3::determinant 
1
Matrix3::trace 
250
2
Matrix3::invert_to_with_det 
Matrix3::diag
1
4
Matrix3::saxpy
Matrix3::multiply_mm 
Matrix3::multiply_mtm 
Matrix3::double_symmetrify 	
Matrix3::inner_product 
3
Matrix3::trace
Matrix3::diag 
5
4
Matrix3::saxpy
Matrix3::saxpy
Matrix3::add
Matrix3::multiply_mtm
Matrix3::multiply_mtm    Matrix3::multiply_mtm )

774

*/
int ConsistentVerondaElasticForceDerivative (TetrahedronState const*me,
											 Matrix3 *dforce,
											 Matrix3 *force,
											 Matrix3 const &disp_mat,
											 Matrix3 const &dir_mat)
{
	Matrix3 C;
	Matrix3 C_inv;
	Matrix3 F;

	Matrix3::MultiplyMM (F, disp_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	Real J = F.Determinant ();
	Real I3 = Sqr(J);
	Real I1 = C.Trace ();
	Real exponent = exp (lame_gamma * (I1 - 3));

	Matrix3::InvertToWithDet (C_inv, C, I3);

	Matrix3 S;
	S.Diag (lame_mu*  exponent);

	Matrix3::Saxpy (S, lame_lambda * (I3-1)*I3 - lame_mu, C_inv, S);

	int flops = 774;
	if (force)
	{
		*force = F * S * me->invloc_transpose_;
		Matrix3::Negate (*force, *force);

		flops += 117;
	}

	Matrix3 DF;
	Matrix3::MultiplyMM (DF, dir_mat, me->inverse_location_);

	Matrix3 DC;
	Matrix3::MultiplyMTM (DC, F, DF);

	Matrix3::DoubleSymmetrify (DC, DC);	

	Matrix3 DS;
	Real cinv_dc = Matrix3::InnerProduct (DC, C_inv);

	Real exp_factor = lame_gamma  * exponent * lame_mu * DC.Trace();
	DS.Diag (exp_factor);
	Matrix3::Saxpy (DS, 0.5 * lame_lambda * (2 * J - 1) * J  * cinv_dc, C_inv, DS);
	Matrix3::Saxpy (DS, lame_mu - lame_lambda* (J-1)*J, C_inv * DC* C_inv, DS);

	*dforce = (DF * S + F * DS) * me->invloc_transpose_;
	Matrix3::Scale (*dforce, -1.0 , *dforce);

	return flops;  
}
